/*
 * @FileName: mppt.c
 * @Author: GreyQiu qiushaogui@aikosolar.com
 * @Date: 2023-07-21 11:51:16
 * @LastEditors: GreyQiu qiushaogui@aikosolar.com
 * @LastEditTime: 2023-09-18 09:02:29
 * @Description: 
 * 
 * Copyright (c) 2023, All Rights Reserved. 
 */

#include "mppt.h"
#include "bsp.h"
#include "math.h"

/**
 * @description: MPPT追踪算法-扰动观察法
 * @param mppt MPPT对象
 * @return 无
 */
static bool mppt_track(mppt_t * mppt, float vin, float iin)
{
    bool rtn = false;
    if (mppt->find_mpp) { // 找到最大功率点
        mppt->find_mpp = false;
        // return true;
    }

//    float iin = *(mppt->iin_addr);
//    float vin = *(mppt->vin_addr);
    float pwr = vin * iin;              // 计算当前功率
    float deltaP = pwr - mppt->pwr_pre; // 计算上一步扰动的deltaP
    int16_t dir = mppt->dir[0];          // 获取前一次的扰动方向

    if (deltaP < 0) { // 观察上一步扰动结果，确定扰动方向
        dir = -dir;
    }

    // 输出超出范围，必须往回扰动
    if (mppt->out >= mppt->outMax) {
        dir = -1;
    } else if (mppt->out <= mppt->outMin) {
        dir = 1;
    }

//    if (deltaP < 0.2f && deltaP > -0.2f) { // 上一次扰动功率变化太小
//        // 功率变化不够大且步长没有展开到最大，都允许加大步长
//        mppt->step = mppt->step + mppt->stepGrad;
//        if (mppt->step > mppt->stepMax) {
//            mppt->step = mppt->stepMax;
//        }
//    } else if (deltaP > 0.6f || deltaP < -0.6f) { // 没有找到最大功率点，逐步加大扰动步长
//        // 功率变化太大
//        mppt->step = mppt->step - mppt->stepGrad;
//        if (mppt->step < mppt->stepMin) {
//            mppt->step = mppt->stepMin;
//        }
//    }

    // 施加扰动
    mppt->out = mppt->out + mppt->step * dir;
    (mppt->loop->vin.ref) = mppt->out;

    // 两步一回头，说明找到最大功率点
    if (mppt->dir[1] == mppt->dir[0] && dir == -mppt->dir[0]) {
        mppt->find_mpp = true;
        rtn = true;
    }

    if (mppt->find_mpp) { // 找到最大功率点，逐步减小扰动步长
        mppt->cnt1 = 8;
        // 功率变化足够大才允许减小步长
        mppt->step = mppt->step - mppt->stepGrad;
        if (mppt->step < mppt->stepMin) {
            mppt->step = mppt->stepMin;
        }
    } else { // 没有找到最大功率点，逐步加大扰动步长
        if (mppt->cnt1 > 0) {
            mppt->cnt1--;
        } else {
            // 功率变化不够大且步长没有展开到最大，都允许加大步长
            mppt->step = mppt->step + mppt->stepGrad;
            if (mppt->step > mppt->stepMax) {
                mppt->step = mppt->stepMax;
            }
        }
    }

    // 更新历史信息，供下次扰动方向的判断
    mppt->pwr_pre = pwr;
    mppt->dir[1] = mppt->dir[0];
    mppt->dir[0] = dir;

    return rtn;
}

/**
 * @description: mppt复位
 * @param mppt
 * @return 
 */
void mppt_reset(mppt_t * mppt, float ref, int16_t dir, float pwr)
{
    mppt->step = mppt->stepMax;
    // 施加扰动
    mppt->out = ref;

    if (mppt->out > mppt->outMax) {
        mppt->out = mppt->outMax;
    } else if (mppt->out < mppt->outMin) {
        mppt->out = mppt->outMin;
    }
    (mppt->loop->vin.ref) = mppt->out;
    // 更新历史信息，供下次扰动方向的判断
    mppt->pwr_pre = pwr;
    mppt->dir[1] = 0;
    mppt->dir[0] = dir;
    mppt->cnt1 = 0;
    mppt->find_mpp = 0;
}

/**
 * @description: MPPT处理函数
 * @param mppt MPPT对象
 * @return 无
 */
void mppt_handle(mppt_t * mppt)
{
    #define DELAY_MPP       (500 / 1)       // MPP点停留周期计数
    #define DELAY_BYPASS    (20 / 1)      // 直通停留周期计数

    if (mppt->cnt3 >= 50) {
        mppt->vinsum += *(mppt->vin_addr);
        mppt->iinsum += *(mppt->iin_addr);
        mppt->loopSum += mppt->loop->vin.Yn_1;
    }
    // 运行周期分频
    if (++(mppt->cnt3) < mppt->div) {
        return;
    }
    uint16_t divnum = mppt->cnt3 - 50; // 共计数
    float vin = mppt->vinsum / divnum;
    float iin = mppt->iinsum / divnum;
    float loopOut = mppt->loopSum / divnum;
    mppt->vinsum = 0;
    mppt->iinsum = 0;
    mppt->loopSum = 0;
    mppt->cnt3 = 0;


//    mppt->pwr = vin * iin;              // 计算当前功率

//    // 更新VOC
//    if (pwr < 6.0f) { // 占空比很小且输出功率很小
//        mppt->voc = vin;
//        mppt->out = vin * 0.9f;
//        *(mppt->ref) = mppt->out;
//        mppt->outMax = vin * 0.9f;
//    } else if (vin > mppt->voc) { // 电池板电压增大
//        mppt->voc = vin;
//        mppt->outMax = vin * 0.9f;
//    }
    float verr = mppt->out - vin;
    switch (mppt->state)
    {
        case MPPT_STATE_DISABLE:
            break;

        case MPPT_STATE_TRACK:
        {
            // 防止某些情况 基准过大导致输入环向下饱和
            if (loopOut < (mppt->loop->vin.Ymin + 0.01f)) {
                mppt_reset(mppt, vin - 0.2f, -1, vin * iin);
                mppt->state = MPPT_STATE_SATURATION_DOWN; // 饱和
                mppt->cnt2 = 0;
            }
//            // 如果MPPT向上饱和
            else if (loopOut > (mppt->loop->vin.Ymax - 0.01f)) {
//                if (verr < -0.5f) {
                    mppt_reset(mppt, vin - 0.5f, -1, vin * iin);
//                }
                mppt->state = MPPT_STATE_SATURATION_UP; // 饱和
                mppt->cnt2 = 0;
            }
            else if (mppt_track(mppt, vin, iin) == true) { // 找到mppt点,会多停留一个MPPT周期
                mppt->state = MPPT_STATE_DELAY; // 在MPP点处停留，以提高MPPT效率
                mppt->cnt2 = 0;
            }
            printf("MPPT_STATE_TRACK, mppt_id = %d, ref = %f\r\n\n", mppt->id, mppt->out);
//            printf("ref = %f\n", mppt->out);
            break;
        }

        case MPPT_STATE_DELAY:
            if (++mppt->cnt2 > 0) { // MPP停留周期数为DELAY_MPP + 2
                mppt->cnt2 = 0;
                mppt->state = MPPT_STATE_TRACK; // 返回MPPT追踪态
            }
            printf("MPPT_STATE_DELAY, mppt_id = %d, ref = %f\r\n\n", mppt->id, mppt->out);
            break;

        case MPPT_STATE_SATURATION_UP:
            if (loopOut < (mppt->loop->vin.Ymax - 0.06f)) { // 退保和
                mppt->state = MPPT_STATE_TRACK; // 饱和
                mppt->cnt2 = 0;
            }
            printf("MPPT_STATE_SATURATION_UP, mppt_id = %d, ref = %f\r\n\n", mppt->id, mppt->out);
            break;

        case MPPT_STATE_SATURATION_DOWN:
            if (loopOut > (mppt->loop->vin.Ymin + 0.06f)) { // 退保和
                mppt->state = MPPT_STATE_TRACK; // 饱和
                mppt->cnt2 = 0;
            }
            printf("MPPT_STATE_SATURATION_DOWN, mppt_id = %d, ref = %f\r\n\n", mppt->id, mppt->out);
            break;
        default:
            break;
    }
}
